#ifndef NV_MATRIX_3X3_H
#define NV_MATRIX_3X3_H
#include <cmath>

//matrix3 is similar with matrix4
namespace nv {
	template<class T>

	class matrix3
	{

	public:

		matrix3() { make_identity(); }

		T & element (int row, int col) {
			return _array[row * 3 + col];
		}

		const T & element (int row, int col) const {
			return _array[row * 3 + col];
		}

		T & operator () (int row, int col) {
			return element(row,col);
		}

		const T & operator () (int row, int col) const {
			return element(row,col);
		}

		void make_identity() {
			element(0,0) = 1.0;
			element(0,1) = 0.0;
			element(0,2) = 0.0; 

			element(1,0) = 0.0;
			element(1,1) = 1.0; 
			element(1,2) = 0.0;

			element(2,0) = 0.0;
			element(2,1) = 0.0;
			element(2,2) = 1.0;
		}

		// dst = M * src
		vec3<T> operator *( const vec3<T> &src) const {
			vec3<T> r;
			for ( int i = 0; i < 3; i++)
				r[i]  = ( src[0] * element(i,0) + src[1] * element(i,1) +
				src[2] * element(i,2));
			return r;
		}

		friend matrix3 inverse( const matrix3 & m) {
			matrix3 minv;

			T r1[6], r2[6], r3[6];
			T *s[3], *tmprow;

			s[0] = &r1[0];
			s[1] = &r2[0];
			s[2] = &r3[0];

			register int i,j,p,jj;
			for(i=0;i<3;i++) {
				for(j=0;j<3;j++) {
					s[i][j] = m.element(i,j);
					if(i==j) s[i][j+3] = 1.0;
					else     s[i][j+3] = 0.0;
				}
			}
			T scp[3];
			for(i=0;i<3;i++) {
				scp[i] = T(fabs(s[i][0]));
				for(j=1;j<3;j++)
					if(T(fabs(s[i][j])) > scp[i]) scp[i] = T(fabs(s[i][j]));
				if(scp[i] == 0.0) return minv; // singular matrix!
			}

			int pivot_to;
			T scp_max;
			for(i=0;i<3;i++) {
				// select pivot row
				pivot_to = i;
				scp_max = T(fabs(s[i][i]/scp[i]));
				// find out which row should be on top
				for(p=i+1;p<3;p++)
					if (T(fabs(s[p][i]/scp[p])) > scp_max) {
						scp_max = T(fabs(s[p][i]/scp[p]));
						pivot_to = p;
					}
					// Pivot if necessary
					if(pivot_to != i) {
						tmprow = s[i];
						s[i] = s[pivot_to];
						s[pivot_to] = tmprow;
						T tmpscp;
						tmpscp = scp[i];
						scp[i] = scp[pivot_to];
						scp[pivot_to] = tmpscp;
					}

					T mji;
					// perform gaussian elimination
					for(j=i+1;j<3;j++) {
						mji = s[j][i]/s[i][i];
						s[j][i] = 0.0;
						for(jj=i+1;jj<6;jj++)
							s[j][jj] -= mji*s[i][jj];
					}
			}
			if(s[2][2] == 0.0) return minv; // singular matrix!

			//
			// Now we have an upper triangular matrix.
			//
			//  x x x | y y y
			//  0 x x | y y y 
			//  0 0 x | y y y
			//
			//  we'll back substitute to get the inverse
			//
			//  1 0 0 | z z z
			//  0 1 0 | z z z
			//  0 0 1 | z z z
			//

			T mij;
			for(i=2;i>0;i--) {
				for(j=i-1;j > -1; j--) {
					mij = s[j][i]/s[i][i];
					for(jj=j+1;jj<6;jj++)
						s[j][jj] -= mij*s[i][jj];
				}
			}

			for(i=0;i<3;i++)
				for(j=0;j<3;j++)
					minv(i,j) = s[i][j+3] / s[i][i];

			return minv;
		}



		union {
			struct {
				T _11, _12, _13;   // standard names for components
				T _21, _22, _23;   // standard names for components
				T _31, _32, _33;   // standard names for components
			};
			T _array[9];     // array access
		};
	};
};


#endif